/*
  File:    band.cpp
  Purpose: Define a band matrix class that's useful for 
           solving large linear systems. Much better 
		  memory use.

  Contact: Paul Macklin
           pmacklin@math.uci.edu
		  http://math.uci.edu/~pmacklin
*/

#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <ctime>
#include <iostream>

using namespace std;

#include "ImprovedMath.h"
#include "random.h"
#include "Matrix.h"
#include "band.h"

bool bMethodFailed = false;
extern double _EPS;

Matrix Perturb( Matrix &In, double Size )
{
 int i;
 Matrix Out = In; 
 
 for( i=0 ; i < In.TellCols() ; i++ )
 {
  double random = 2.0 * UniformRandom() - 1.0;
  double addon = random * Size;
  *Out(i) += addon; 
 }
 
 return Out;
}

void swap(double *dIn1, double *dIn2)
{
 double dTemp = *dIn1;
 *dIn1 = *dIn2;
 *dIn2 = dTemp;
}

  
BandMatrix::BandMatrix()
{
 Mat = Matrix();
 Rows = 1;
 Cols = 1;
 nLowerBandwidth = 0;
 nUpperBandwidth = 0;
 /* 
   New for more efficient multiplications
 */
 NonzeroBandsBinary = zeros(1,nLowerBandwidth+nUpperBandwidth+1);
 NonzeroBandsList = zeros(1,nLowerBandwidth+nUpperBandwidth+1);
 nNumberOfNonzeroBands = 0;
 bIsCompacted = false;
}

BandMatrix::BandMatrix(int nR, int nC, int nLBW, int nUBW)
{
 Rows = nR;
 Cols = nC;
 Mat = zeros(nR, nLBW + nUBW + 1);
 nLowerBandwidth = nLBW;
 nUpperBandwidth = nUBW;
 /* 
   New for more efficient multiplications
 */
 NonzeroBandsBinary = zeros(1,nLowerBandwidth+nUpperBandwidth+1);
 NonzeroBandsList = zeros(1,nLowerBandwidth+nUpperBandwidth+1);
 nNumberOfNonzeroBands = 0;     
 bIsCompacted = false;
}
  
BandMatrix::BandMatrix(int nR, int nC, int nLBW, int nUBW, Matrix& NzBLt)
{
 Rows = nR;
 Cols = nC;
 Mat = zeros( nR , (int) NzBLt.TellCols() );
 nLowerBandwidth = nLBW;
 nUpperBandwidth = nUBW;
 NonzeroBandsBinary = zeros(1,nLowerBandwidth+nUpperBandwidth+1);
 NonzeroBandsList = zeros(1,nLowerBandwidth+nUpperBandwidth+1);
 nNumberOfNonzeroBands = NzBLt.TellCols();
 bIsCompacted = true;
 
 int i=0;
 for( i=0 ; i < nNumberOfNonzeroBands ; i++)
 {
  *NonzeroBandsList(i) = *NzBLt(i); 
  *NonzeroBandsBinary( (int) *NzBLt(i) ) = 1.0;
 }
 return;
}

double* BandMatrix::e(int nI, int nJ)
{
 int nITemp=0;
 int nJTemp=0;
 int k = nJ - nI + nLowerBandwidth;
 if( 0 <= k && k <= nLowerBandwidth + nUpperBandwidth )
 {
  nITemp = nI ; 
  nJTemp = k;
  /*
    New for more efficient multiplication
  */
  *NonzeroBandsBinary(k) = 1;
 }   
 
 if( bIsCompacted )
 {
  int nTemp = 0;
  while( *NonzeroBandsList(nTemp) < nJTemp )
  { nTemp++; }
  nJTemp = nTemp;
 }
 return Mat(nITemp, nJTemp);
}
  
double* BandMatrix::eNoUpdate(int nI, int nJ)
{
 int nITemp=0;
 int nJTemp=0;
 int k = nJ - nI + nLowerBandwidth;
 if( 0 <= k && k <= nLowerBandwidth + nUpperBandwidth )
 {
  nITemp = nI ; 
  nJTemp = k;
 }

 if( bIsCompacted )
 {
  int nTemp = 0;
  while( *NonzeroBandsList(nTemp) < nJTemp )
  { nTemp++; }
  nJTemp = nTemp;
 }  
 return Mat(nITemp, nJTemp);
}

void BandMatrix::Compactify(void)
{
 if( bIsCompacted )
 { return; }
 Matrix mTemp = zeros(Rows,nNumberOfNonzeroBands );
 int i,j;
 for( i=0 ; i < Rows ; i++)
 {
  for( j=0 ; j < nNumberOfNonzeroBands ; j++)
  {
   *mTemp(i,j) = *Mat(i,(int) *NonzeroBandsList(0,j) );
  }
 }
 Mat = mTemp;
 bIsCompacted = true;
 return;
}

void BandMatrix::Decompactify(void)
{
 if( !bIsCompacted )
 { return; }
 Matrix mTemp = zeros(Rows,nUpperBandwidth+nLowerBandwidth+1 );
 int i,j;
 for( i=0 ; i < Rows ; i++)
 {
  for( j=0 ; j < nNumberOfNonzeroBands ; j++)
  {
   *mTemp(i,(int) *NonzeroBandsList(0,j)) = *Mat(i,j );
  }
 }
 Mat = mTemp;
 bIsCompacted = false;
 return;
}




void BandMatrix::display(void)
{
 cout << "Lower Bandwidth: " << nLowerBandwidth << "\t"
      << "Upper Bandwidth: " << nUpperBandwidth << "\n";
 Mat.display();
 return;
}

void BandMatrix::CleanupBandMatrix(void)
{
 int i=0,j=0;
 for(i= nLowerBandwidth-1 ; i >= 0 ; i--)
 {
  for(j= nLowerBandwidth-i-1 ; j >= 0 ; j--)
  {
   *Mat(i,j) = 0.0;
  }
 }

 for( i=0 ; i < nUpperBandwidth ; i++)
 {
  for( j= nLowerBandwidth+i+1; j < Mat.TellCols() ; j++)
  {
   *Mat(Rows-1-i, j) = 0.0;
  }
 }
 return;
}

BandMatrix transpose(BandMatrix &mIn)
{
 BandMatrix bmOut(mIn.TellRows(),mIn.TellCols(),mIn.nUpperBandwidth,mIn.nLowerBandwidth);
 int i=0,j=0;
 for(i=0;i<mIn.TellRows();i++)
 {
  int nStart = (int) MAX(0,i-mIn.nLowerBandwidth);
  int nEnd = (int) MIN(mIn.TellCols()-1,(mIn.Mat).TellCols()-1+i-mIn.nLowerBandwidth);
  for(j=nStart;j<=nEnd;j++)
  {
   int k = j - i + mIn.nLowerBandwidth;  
   if( *mIn.NonzeroBandsBinary(k) == 1 )
   {
    *bmOut.e(j,i) = *mIn.eNoUpdate(i,j);
   }
  }
 }

 return bmOut;
}

Matrix BandMatrix::BandMultiplyCompacted(Matrix *mIn)
{
 Matrix mOut = zeros(1,Rows);
 int i=0;
 int nStart;
 int nEnd;

 for(i=0;i<nLowerBandwidth;i++)
 {
  int k=nNumberOfNonzeroBands-1;
  int nMatrixIndex = (int) *NonzeroBandsList(k);
  long double ldTemp = 0.0;
  while( nMatrixIndex >= nLowerBandwidth-i && k > 0)
  {
   nMatrixIndex = (int) *NonzeroBandsList(k);
   int nVectorIndex = nMatrixIndex + i - nLowerBandwidth;
   int nTemp = nMatrixIndex; // added for the compact version
   nMatrixIndex = k; // added for the compact version
   if( nVectorIndex < 0)
   {
    break;
   }

   ldTemp = ldTemp + (long double) ( (*Mat(i,nMatrixIndex)) * 
                                       (*(*mIn)(nVectorIndex)) );
   nMatrixIndex = nTemp; // added for compact version
   k--;
  }
  *mOut(i) = (double) ldTemp;
 
 }
 for(i=nLowerBandwidth;i<Rows-nUpperBandwidth;i++)
 {
  int k=0;
  long double ldTemp = 0.0;
  for(k=0;k<nNumberOfNonzeroBands;k++)
  {
   int nMatrixIndex = (int) *NonzeroBandsList(k);
   int nVectorIndex = nMatrixIndex + i - nLowerBandwidth;
   nMatrixIndex = k; // added for compact version
   ldTemp = ldTemp + ( *Mat(i,nMatrixIndex) ) * ( *(*mIn)(nVectorIndex) );
  }
  *mOut(i) = (double) ldTemp;
 }
 
 for(i=Rows-nUpperBandwidth;i<Rows;i++)
 {

  int k=0;
  int nMatrixIndex = (int) *NonzeroBandsList(k);
  long double ldTemp = 0.0;
  while( nMatrixIndex <= i+nUpperBandwidth && k < nNumberOfNonzeroBands)
  {
   nMatrixIndex = (int) *NonzeroBandsList(k);
   int nVectorIndex = nMatrixIndex + i - nLowerBandwidth;
   if( nVectorIndex >= (*mIn).TellCols() )
   {
    break;
   }
   int nTemp = nMatrixIndex; // added for compact version
   nMatrixIndex = k; // added for compact version
   ldTemp = ldTemp + (long double) ( (*Mat(i,nMatrixIndex)) * 
                                       (*(*mIn)(nVectorIndex)) );
   nMatrixIndex = nTemp; // added for compact version 
   k++;
  }
  *mOut(i) = (double) ldTemp;

 }
 return mOut;
}

Matrix BandMatrix::BandMultiplyUncompacted(Matrix *mIn)
{
 Matrix mOut = zeros(1,Rows);
 int i=0;
 int nStart;
 int nEnd;

 for(i=0;i<nLowerBandwidth;i++)
 {
  int k=nNumberOfNonzeroBands-1;
  int nMatrixIndex = (int) *NonzeroBandsList(k);
  long double ldTemp = 0.0;
  while( nMatrixIndex >= nLowerBandwidth-i && k > 0)
  {
   nMatrixIndex = (int) *NonzeroBandsList(k);
   int nVectorIndex = nMatrixIndex + i - nLowerBandwidth;
   if( nVectorIndex < 0)
   {
    break;
   }

   ldTemp = ldTemp + (long double) ( (*Mat(i,nMatrixIndex)) * 
                                       (*(*mIn)(nVectorIndex)) );
   k--;
  }
  *mOut(i) = (double) ldTemp;
 
 }
 for(i=nLowerBandwidth;i<Rows-nUpperBandwidth;i++)
 {
  int k=0;
  long double ldTemp = 0.0;
  for(k=0;k<nNumberOfNonzeroBands;k++)
  {
   int nMatrixIndex = (int) *NonzeroBandsList(k);
   int nVectorIndex = nMatrixIndex + i - nLowerBandwidth;
   ldTemp = ldTemp + ( *Mat(i,nMatrixIndex) ) * ( *(*mIn)(nVectorIndex) );
  }
  *mOut(i) = (double) ldTemp;
 }
 
 for(i=Rows-nUpperBandwidth;i<Rows;i++)
 {

  int k=0;
  int nMatrixIndex = (int) *NonzeroBandsList(k);
  long double ldTemp = 0.0;
  while( nMatrixIndex <= i+nUpperBandwidth && k < nNumberOfNonzeroBands)
  {
   nMatrixIndex = (int) *NonzeroBandsList(k);
   int nVectorIndex = nMatrixIndex + i - nLowerBandwidth;
   if( nVectorIndex >= (*mIn).TellCols() )
   {
    break;
   }
   ldTemp = ldTemp + (long double) ( (*Mat(i,nMatrixIndex)) * 
                                       (*(*mIn)(nVectorIndex)) );
   k++;
  }
  *mOut(i) = (double) ldTemp;

 }
 return mOut;
}

Matrix BandMatrix::BandMultiply( Matrix *mIn)
{
 if( bIsCompacted )
 { return BandMultiplyCompacted( mIn ); }
 else
 { return BandMultiplyUncompacted( mIn ); }
}

void BandMatrix::UpdateNonzeroBandsList(void)
{
 int k=0, j=0;
 nNumberOfNonzeroBands = 0;
 for(j=0;j<=nLowerBandwidth+nUpperBandwidth;j++)
 {
  if( *NonzeroBandsBinary(j) > 0.0 )
  {
   *NonzeroBandsList(k) = j;
   k++;
   nNumberOfNonzeroBands++;
  }
 }
 
 return;
}

Matrix CreateInitialGuess( Matrix *mIn)
{
 int MM = (*mIn).TellRows();
 int NN = (*mIn).TellCols();
 int nTopIndex = (MM-2)*(NN-2);
 int nOffset = (MM-2);

 Matrix vOut = zeros(1, nTopIndex); 
 int i,j;
 for( i=1; i < MM-1 ; i++)
 {
  for( j=1; j < NN-1 ; j++)
  {
   int n = (j-1)*nOffset + (i-1) ;
   *vOut(n) = *(*mIn)(i,j);
  }
 }

 return vOut;
}

void BandMatrix::bandBiCGsolve(BandMatrix *bmTranspose, Matrix *vRHS, 
                               Matrix *vInitialGuess, double dTolerance, 
                               int nMaxIterations)
{
 Decompactify();
 bMethodFailed = false;

 Matrix vR =  *vRHS - BandMultiply( vInitialGuess);
 
 if( vR.InfinityNorm() < dTolerance )
 {
  cout << "BiCG: Initial Guess satisfied tolerance.  Quitting!\n";
  return;
 }
 cout << "done\n";
 Matrix vRbar = vR;
 
 Matrix vZ;
 Matrix vZbar;
 
 Matrix vP;
 Matrix vPbar;
 
 Matrix vQ;
 Matrix vQbar;

 int i=1;
 bool bStop = false;
 
 double dRho;
 double dRhoPrev;
 double dBeta;
 double dAlpha;
 
 while(  i <= nMaxIterations && bStop == false)
 {
  /*
    Pre-conditioning can go here later.  For now, 
    K = I.
  */
  
  vZ = vR;
  vZbar = vRbar;
  
  dRho = DotProduct( vRbar, 0, 0, vZ, 0, 0, vZ.TellCols()-1);
  if( i == 1)
  {
   vP = vZ;
   vPbar = vZbar;
  }
  else
  {
   dBeta = dRho / dRhoPrev;
   vP = vZ + dBeta * vP;
   vPbar = vZbar + dBeta * vPbar;
  }
  
  vQ = BandMultiply( &vP);
  vQbar = (*bmTranspose).BandMultiply( &vPbar);
  
  double dTemp = DotProduct( vPbar, 0, 0, vQ, 0, 0, vQ.TellCols()-1);
  if( fabs( dTemp ) <= _EPS )
  {
   cout << "BiCG Error: Division by zero in alpha.  Terminating ... \n";
   bMethodFailed = true;
   return;
  }
  dAlpha = dRho / dTemp;
  *vInitialGuess = *vInitialGuess + dAlpha* vP;
  vR = vR - dAlpha * vQ;
  vRbar = vRbar - dAlpha * vQbar;

  dRhoPrev = dRho; 
  
  if( vR.InfinityNorm() < dTolerance )
  {
   bStop = true;
   cout << "BiCG: Successful convergence with infinity norm " << vR.InfinityNorm() << " after " 
        << i+1 << " iterations!\n";
  }
  else
  {
   if( fabs( dRhoPrev ) <= _EPS )
   {
    cout << "BiCG Error: Division by zero in beta. Terminating ... \n";
    bMethodFailed = true;
    return;
   }
   i++;
  }
 }
 
 if( bStop == true )
 {
//  cout << "Tolerance reached after " << i << " iterations!\n";
 }
 else
 {
  cout << "BiCG: I gave up after " << i << " iterations.\n";
 }
 return;
}


/*
  This is based on the bandec algorithm in Numerical Recipes for C, 
  page 53, only with readable code instead.  A tip to Press, Teukolsky, 
  Vetterling, and Flannery: never use a lowercase "l" as a variable; it 
  looks too much like a one "1" on many platforms, including your own book.

  Tip to the authors: Maybe your publishers love you for your compact lack 
  of indentation and braces in logical structures, but your readers (i.e. 
  paying customers) don't. 
*/

void BandMatrix::bandec( BandMatrix *bmL, int nIndex[], double *dOut)
{
 int i, j, k, L, mm; 
 double dTemp; 
 mm = nLowerBandwidth + nUpperBandwidth + 1;
 L = nLowerBandwidth;

 /*
   Rearrange the first nLowerBandwidth rows so that the zeros 
   appear in the last several columns, rather than the first 
   several columns.  

   This is done because the final result will be upper-triangular. 
 */

 for(i=0 ; i < nLowerBandwidth ; i++)
 {
  for( j=nLowerBandwidth - i ; j < mm ; j++)
  {
   *Mat(i,j-L) = *Mat(i,j);
  }
  L--; 
  for( j=mm-L-1 ; j < mm ; j++)
  {
   *Mat(i,j) = 0.0;
  }
 }

 *dOut = 1.0;
 L = nLowerBandwidth;

 /*
   Now proceed row-wise.
 */

 for( k=0 ; k < Rows ; k++)
 {
  dTemp = *Mat(k,0);
  i=k;
  if( L < Rows )
  {
   L++;
  }

  /*
    Do partial pivoting; find the pivot element.
  */

  for( j=k+1 ; j < L ; j++)  // j < L
  {
   if( fabs(*Mat(j,0)) > fabs(dTemp) )
   {
    dTemp = *Mat(j,0) ; 
    i = j;
   }
  }
  nIndex[k] = i;
 
  /*
    The matrix is now numerically unstable. (Tip to the authors: 
    equality rarely happens for doubles and floats, so don't use 
    dTemp == 0.0. Duh.)  Proceed anyway, as it can still be useful. 
    Substitute in _EPS for the small value.
  */

  if( fabs( dTemp - 0.0) <= _EPS ) 
  {
   *Mat(k,0) = _EPS;
  }

  /*
    Interchange rows if necessary
  */

  if( i != k)
  {
   *dOut = -*dOut;
   for(j=0 ; j < mm ; j++)
   {
    swap( Mat(k,j) , Mat(i,j) );
   }
  }
 
  /* 
    Do the elimination.  
  */


  for(i = k+1 ; i < L ; i++) // i=k+1 : ..
  {
   dTemp = *Mat(i,0) / *Mat(k,0);
   *( ( (*bmL).Mat)(k,i-k-1) )= dTemp;
   for(j=1; j < mm ; j++)
   {
    *Mat(i,j-1) = *Mat(i,j) - dTemp * (*Mat(k,j));
   }
   *Mat(i,mm-1) = 0.0;
  }

 }  
 nUpperBandwidth = nUpperBandwidth + nLowerBandwidth;
 nLowerBandwidth = 0;
}

/*
  This is based on the banbks algorithm in Numerical Recipes for C, 
  page 54, only with readable code instead. 
*/

void BandMatrix::banbks( BandMatrix *bmL, int nIndex[] , Matrix *mRHS)
{
 /*
   Assumes that Mat has been converted to the U of the banded 
   LU decomposition and that, bmL is the L of that algorithm, 
   nIndex is the index of row permutations, and mRHS is the 
   RHS of the system you want to solve.  It will be overwritten 
   with the solution.  
 */

 int i, k, L;
 int mm;
 double dTemp;
 long double ldTemp;

 mm = nUpperBandwidth + 1; // recall that the bandwidth of the 
                          // overwritten A is the sum of its 
                          // old upper and lower bandwidths
 L = (*bmL).nLowerBandwidth;  // recall that bmL has the 
                              // same lower bandwidth as the 
                              // original A

 /*
   Forward substitution, heeding the row permutations indicated 
   by nIndex as we proceed. 
 */

 for( k=0 ; k < Rows ; k++)
 {
  i = nIndex[k];
  if( i != k )
  {
   swap( (*mRHS)(k), (*mRHS)(i)  );
  }
  if( L < Rows )
  {
   L++;
  }
  for(i=k+1 ; i < L ; i++)
  {
   *(*mRHS)(i) = *(*mRHS)(i) - ( *( (*bmL).Mat(k,i-k-1)) )*(*(*mRHS)(k));
   // Directly access the lower-triangular matrix because they store it kinda' screwy
  }  
 }

 L = 1;
 
 /*
   back substitution
 */
 
 for(i=Rows; i >= 1 ; i--)
 {
  ldTemp = (long double) *(*mRHS)(i-1);
  for( k=2; k <= L ; k++)
  {
   ldTemp = ldTemp - (long double) (  (*Mat(i-1,k-1))*(*(*mRHS)(k+i-2))   );
  }
  dTemp = (double) ldTemp;
  *(*mRHS)(i-1) = dTemp / (*Mat(i-1,1-1));
  if(L < mm )
  { 
   L++;
  }
 }
}


void BandMatrix::bandlinsolve( Matrix *mRHS)
{
 Decompactify();
 BandMatrix Lower(Rows, Cols, nLowerBandwidth , 0);
 int *nIndex;
 double *dOut;
 dOut = new double;
 nIndex = new int [Rows];
 bandec( &Lower, nIndex , dOut );
 banbks( &Lower, nIndex , mRHS);

 /*
   Clean up memory
 */
 Mat = Matrix(1,1);
 Rows = 1; 
 Cols = 1; 
 nLowerBandwidth = 0;
 nUpperBandwidth = 0;
 Lower = BandMatrix(1,1,0,0);
}

void BandMatrix::bandlinsolveBiCG( Matrix *mRHS, Matrix *vInit)
{
 Decompactify();
 UpdateNonzeroBandsList();

 BandMatrix Atranspose = transpose( *this);
 Atranspose.UpdateNonzeroBandsList();

 double dTolerance = 1e-8; // pow(dDeltaX, 4.0);
 int nMaxIterations = 10000;
 
 BandMatrix::bandBiCGsolve( &Atranspose, mRHS, vInit, 
                            dTolerance, nMaxIterations);
 
 /*
   Clean up memory
 */
 if( bMethodFailed == false )
 {
  Mat = Matrix(1,1);
 }
 else
 {
  cout << "BiCG: Using backup method: banded Gaussian elimination ... \n";
  bandlinsolve( mRHS);
  *vInit = *mRHS;
 }
}

double BandMatrix::bandlinsolveBiCGStabOf2( Matrix *vRHS, Matrix *vInitialGuess , 
                                            double dTolerance , int nMaxIterations )
{
 UpdateNonzeroBandsList();
 Compactify();
 
 // new on 10-13-2004:
 // keep track of 5 most recent errors
 double Errors[5];
 Errors[0] = 9e99;
 Errors[1] = 8e99;
 Errors[2] = 7e99;
 Errors[3] = 6e99;
 Errors[4] = 5e99;
  
 Matrix vX = *vInitialGuess;
 Matrix vR0 = *vRHS - BandMultiply( &vX );
 Matrix vR0hat = vR0;
 
 double dRho0 = 1.0;
 double dAlpha = 0.0;
 double dBeta = 0.0;
 double dGamma = 0.0;
 double dOmega1 = 0.0;
 double dOmega2 = 1.0;
 double dMu;
 double dNu;
 double dTau;
 
 Matrix vU = zeros(1,vX.TellCols());
 Matrix vV;
 Matrix vW;
 Matrix vS;
 Matrix vT;
 
 Matrix vR = vR0;
 Matrix vRhat = vR0hat;
 
 int nCurrentIteration = 0;
 
 bool bDone = false;
 
 bool trouble = false;
 int TroubleCount = 0;
 int MaxTroubleCount = 5;
 double trouble_size = 1e-15;
 double dError;
 
 if( vR0.InfinityNorm() <= dTolerance )
 { bDone = true; }
 
 while( !bDone && nCurrentIteration < nMaxIterations && !trouble )
 {
//  cout << nCurrentIteration << ": ";
  dRho0 = -dOmega2 * dRho0;
  // even BiCG step
  double dRho1 = DotProduct( vR0hat , 0, 0 , 
                             vR , 0, 0, 
                             vR0hat.TellCols() );
  if( fabs( dRho0 ) <= trouble_size )
  { trouble = true; }
  
  dBeta = dAlpha * dRho1 / dRho0; 
  dRho0 = dRho1;                          
  vU = vR - dBeta* vU;
  vV = BandMultiply( &vU );
  dGamma = DotProduct( vR0hat,0,0, vV,0,0, vV.TellCols() );
  if( fabs( dGamma ) <= trouble_size )
  { trouble = true; }
  
  dAlpha = dRho0 / dGamma;
  vR = vR - dAlpha * vV;
  vS = BandMultiply( &vR );
  vX = vX + dAlpha * vU;
  
  // odd BiCG step
  dRho1 = DotProduct( vR0hat,0,0, vS,0,0, vS.TellCols() );
  if( fabs( dRho0 ) <= trouble_size )
  { trouble = true; }
  
  dBeta = dAlpha * dRho1 / dRho0;
  dRho0 = dRho1;
  vV = vS - dBeta*vV;
  vW = BandMultiply( &vV );
  dGamma = DotProduct( vR0hat,0,0, vW,0,0, vW.TellCols() );
  if( fabs( dGamma ) <= trouble_size )
  { trouble = true; }
  
  dAlpha = dRho0 / dGamma;
  vU = vR - dBeta * vU;
  vR = vR - dAlpha * vV;
  vS = vS - dAlpha * vW;
  vT = BandMultiply( &vS );
  // GCR(2) part
  dOmega1 = DotProduct( vR,0,0, vS,0,0, vS.TellCols());
  dMu = DotProduct( vS,0,0, vS,0,0, vS.TellCols());
  dNu = DotProduct( vS,0,0, vT,0,0, vT.TellCols());
  dTau = DotProduct( vT,0,0, vT,0,0, vT.TellCols());
  dOmega2 = DotProduct( vR,0,0, vT,0,0, vT.TellCols());
  dTau = dTau - square( dNu );
  dOmega2 = (dOmega2-dNu*dOmega1/dMu)/dTau;
  dOmega1 = (dOmega1-dNu*dOmega2)/dMu;
  
  if( !trouble )
  {
   vX = vX + dOmega1*vR + dOmega2*vS + dAlpha*vU;
   vR = vR - dOmega1*vS - dOmega2*vT;
   vU = vU - dOmega1*vV - dOmega2*vW;   
   
   TroubleCount = 0;
  }
  else
  {
   TroubleCount++;
   if( TroubleCount <= MaxTroubleCount )
   {
    vX = Perturb( vX, 1e-15 );
    vR = *vRHS - BandMultiply( &vX );
    vU = zeros( 1, vRHS->TellCols() );
    trouble= false;
   }  
  }
  
  Matrix vError = BandMultiply( &vX ) - *vRHS;
  dError = vError.InfinityNorm();
//  cout << " " << dError << "\n";
    
  if( dError <= dTolerance )
  { bDone = true; }
  
  // calculate least squares line of 5 most recent
  int i;
  for( i=0 ; i <= 3 ; i++)
  { Errors[i] = Errors[i+1]; }
  Errors[4] = dError;
  
  double sum1 = 0.0;
  double sum2 = 0.0;
  
  i = 0;
  while( i < 5 )
  {
   sum1 += i * Errors[i];
   sum2 += Errors[i];
//   cout << Errors[i]  << " ";
   i++;
  }
  
  double Slope = (sum1 - 2.0*sum2 )/19.0;
//  cout << ":: slope: " << Slope << "\n";
  
  if( Slope > 0.001 )
  {
   cout << "This sucks.  The errors are getting worse as a trend. I'm outta' here!\n";
   bDone = true;
  }

 
  nCurrentIteration += 2;
 }
 *vInitialGuess = vX;
 return dError;
}

//

void BandMatrix::ThomasSolve( Matrix *vRHS, Matrix *vSolution )
{
 if( nLowerBandwidth < 1 || nUpperBandwidth < 1 )
 {
  cout << "Thomas (conditioning) method error: upper and lower bandwith must be 1 or more! \n"
       << "                                    Bad scientist!  I suggest channeling the spirit of\n"
       << "                                    Dave Thomas from Wendy's.  Mmmm.  Food!\n\n";
  return; 
 }
 double beta;
 Matrix Gamma = zeros(1, vRHS->TellCols());
  
 beta = *eNoUpdate(0,0);
 *(*vSolution)(0) = *(*vRHS)(0) / beta;
 int j;

 for( j=1 ; j<= vRHS->TellCols()-1 ; j++ ) 
 {
  *Gamma(j) = *eNoUpdate(j-1,j) / beta;
  beta = *eNoUpdate(j,j) - ( *eNoUpdate(j,j-1) ) * ( *Gamma(j) );
  *(*vSolution)(j) = ( *(*vRHS)(j) - ( *eNoUpdate(j,j-1) ) * ( *(*vSolution)(j-1) ) ) / beta;
 }
 
 for( j=vRHS->TellCols()-2 ; j >= 0 ; j-- )
 {
  *(*vSolution)(j) = *(*vSolution)(j) - ( *Gamma(j+1) ) * ( *(*vSolution)(j+1) );
 }
 
 return;
}

double BandMatrix::bandlinsolveBiCGStabOfL( Matrix *vRHS, Matrix *vInitialGuess , int L, 
                                            double dTolerance , int nMaxIterations )
{
 int i,j,k;
 
 // scaling
 
 double scale = Mat.InfinityNorm();
 Mat = (1.0/(scale+_EPS)) * Mat;
 *vRHS = (1.0/(scale+_EPS)) * (*vRHS);

 Matrix BoldU = zeros( L+1, vRHS->TellCols() );
 Matrix BoldR = zeros( L+1 , vRHS->TellCols() );
  
 Matrix Temp0 = *vRHS - BandMultiply( vInitialGuess );
 Matrix Temp1 = zeros( 1, Temp0.TellCols() );
 Matrix Temp2 = Temp1;
 
 BoldR.UpdateSubmatrix(Temp0, 0, 0, 0, Temp0.TellCols()-1);
  
 Matrix BoldX = *vInitialGuess;
 Matrix R0tilde = BoldR.submatrix(0,0,0,BoldR.TellCols()-1);
 
 double zeta0 = Temp0.InfinityNorm();
 
 double alpha = 1.0;
 double rho0 = 1.0;
 double omega = 1.0;
 double zeta = zeta0;

 int MaxPerturbations = 25; 
// double temp_eps = 1e-5;
 double temp_eps = dTolerance;
 int iteration = 0;
 int perturbcount = 0;
 double emergency_criterion = 10.0;

 if( zeta < dTolerance )
 {
  return zeta;
 }
 
 Matrix Z = zeros(L,L);
 Matrix y = zeros(1,L);
 
 begin_loop:
 
// while( zeta > temp_eps * zeta0 && iteration < nMaxIterations 
  while( zeta > dTolerance && iteration < nMaxIterations 
        && perturbcount <= MaxPerturbations )
 {
  // BiCG part // 
  rho0 = -omega*rho0;
  for( j=0 ; j <= L-1 ; j++)
  {
   Temp0 = BoldR.submatrix(j,j,0,BoldR.TellCols()-1);
   double rho1 = DotProduct(Temp0,0,0,R0tilde,0,0,Temp0.TellCols());
   
   double beta = alpha * rho1 / rho0;
   
   if( fabs( log10(beta) ) >= emergency_criterion )
   {
    cout << "Emergency stop! (beta)\n"; 
    goto perturb;
   }
   
   rho0 = rho1;
   for( i=0 ; i <= j ; i++) 
   {
    Temp0 = BoldR.submatrix(i,i,0,BoldR.TellCols()-1);
    Temp1 = BoldU.submatrix(i,i,0,BoldU.TellCols()-1);
    Temp2 = Temp0 - beta*Temp1;
    BoldU.UpdateSubmatrix(Temp2,i,i,0,Temp2.TellCols()-1);
   }
   Temp0 = BoldU.submatrix(j,j,0,BoldU.TellCols()-1);
   Temp1 = BandMultiply( &Temp0 );
   BoldU.UpdateSubmatrix( Temp1, j+1,j+1,0,BoldU.TellCols()-1);
   double sigma = DotProduct(Temp1,0,0,R0tilde,0,0,Temp1.TellCols());
    
   alpha = rho1/sigma;
   if( fabs( log10(alpha) ) >= emergency_criterion )
   {
 //   cout << "Emergency stop! (alpha)\n"; 
    goto perturb;
   }   
   
   Temp2 = BoldU.submatrix(0,0,0,BoldU.TellCols()-1);
   BoldX = BoldX + alpha*Temp2;
   *vInitialGuess = BoldX;   
   
   for( i=0 ; i<=j ;i++)
   {
    Temp0 = BoldR.submatrix(i,i,0,BoldR.TellCols()-1);
    Temp1 = BoldU.submatrix(i+1,i+1,0,BoldU.TellCols()-1);
    Temp2 = Temp0 - alpha*Temp1;
    BoldR.UpdateSubmatrix( Temp2, i,i,0,Temp2.TellCols()-1);
   }
   Temp0 = BoldR.submatrix(j,j,0,BoldR.TellCols()-1);
   Temp1 = BandMultiply( &Temp0 );
   BoldR.UpdateSubmatrix(Temp1, j+1,j+1, 0,Temp1.TellCols()-1); 
  }
  // polynomial part
  for( i=1 ; i <= L ; i++)
  {
   Temp0 = BoldR.submatrix(i,i,0,BoldR.TellCols()-1);  
   for( j=1 ; j <= i ; j++)
   {
    Temp1 = BoldR.submatrix(j,j,0,BoldR.TellCols()-1); 
    *Z(i-1,j-1) = DotProduct( Temp0, 0,0, Temp1, 0,0, Temp1.TellCols() );
    *Z(j-1,i-1) = *Z(i-1,j-1);
   }
   Temp1 = BoldR.submatrix(0,0,0,BoldR.TellCols()-1);
   *y(i-1) = DotProduct(Temp0,0,0,Temp1,0,0,Temp1.TellCols());
  }
  Temp0 = y;
  Temp1 = GaussianEliminationPartialPivoting( Z,Temp0 );
  Temp2 = Temp1 - Temp0;
  if( Temp2.InfinityNorm() < 1e-15 )
  {
 //  cout << "Emergency Stop! (GePP)\n";
   goto perturb;
  }
  y = Temp1;
  
  
  omega = *y(L-1);
  for( i=1 ; i <= L ; i++)
  {
   Temp0 = BoldU.submatrix(0,0,0,BoldU.TellCols()-1);
   Temp1 = BoldU.submatrix(i,i,0,BoldU.TellCols()-1);
   Temp2 = Temp0 - (*y(i-1))*Temp1;
   BoldU.UpdateSubmatrix(Temp2,0,0,0,BoldU.TellCols()-1);
   
   Temp2 = BoldR.submatrix(i-1,i-1,0,BoldR.TellCols()-1);
   BoldX = BoldX + (*y(i-1))*Temp2;
   *vInitialGuess = BoldX;   
   
   Temp0 = BoldR.submatrix(0,0,0,BoldR.TellCols()-1);
   Temp1 = BoldR.submatrix(i,i,0,BoldR.TellCols()-1);
   Temp2 = Temp0 - (*y(i-1))*Temp1;
   BoldR.UpdateSubmatrix(Temp2,0,0,0,BoldR.TellCols()-1);   
  }
  Temp0 = BoldR.submatrix(0,0,0,BoldR.TellCols()-1);
  zeta = Temp0.InfinityNorm();
 
//  cout << iteration << " " << zeta << "\n";
  
  iteration++;
 }
 
 // make sure to overwrite initial guess with x
 
/* 
 cout << "summary: \n"
      << "=======\n"
      << "iterations: " << iteration << "\n"
      << "perturbations applied: " << perturbcount << "\n"
      << "infinity-norm of error: " << zeta << "\n"
      << "desired infinity-norm: " << dTolerance << "\n\n";
//      << "2-norm of error: " << zeta << "\n"
//      << "desired 2-norm: " << zeta0*temp_eps << "\n\n";
*/ 
 Mat = scale * Mat;
 *vRHS = scale * (*vRHS);
  
 return zeta;
 
 perturb:
 perturbcount++;
 Temp0 = BoldX;
 if( perturbcount <= MaxPerturbations )
 {
//  cout << "\tPerturbing current guess ... iteration: " << iteration 
//       << " perturbation: " << perturbcount;
  double PerturbSize = (.001) * zeta;
  // double PerturbSize = .001 * Temp0.normInf();
  // double PerturbSize = 1e-15;
//  cout << " size: " << PerturbSize << "\n";
  Temp0 = Perturb( Temp0 , PerturbSize  );
  *vInitialGuess = Temp0;
  BoldX = Temp0;
  Temp0 = *vRHS - BandMultiply( vInitialGuess );
  BoldR.UpdateSubmatrix(Temp0, 0, 0, 0, Temp0.TellCols()-1);
 }
 iteration++;
 goto begin_loop;
 
 return zeta;
}
                                          
int BandMatrix::TellRows( void )
{ return Rows; }

int BandMatrix::TellCols( void )
{ return Cols; }
